#ifndef     __DELAYED__MESSAGE_QUEUE_H
#define     __DELAYED__MESSAGE_QUEUE_H

#include "../utils/priority_list.h"
#include "../rtos/event_flags.h"

namespace rtos {

template <typename EleType, uint32_t Capacity>
class DelayedMessageQueue {
public:
    using NodeType = utils::Node<EleType, uint32_t>;
    static constexpr uint32_t LockFlag = 0x01;
    static constexpr uint32_t NotifyFlag = 0x02;

    DelayedMessageQueue() {
        for (uint32_t i = 0; i < Capacity; i ++) {
            m_free.insert(m_nodes + i);
        }
        m_flags.SetFlags(LockFlag);
    }

    NodeType* Post(const EleType &ele, uint32_t delayMs = 0) {
        lock();
        NodeType *node = m_free.next;
        if (node == nullptr) {
            unlock();
            return nullptr;
        }
        node->ele = ele;
        node->priorityValue = delayMs + osKernelGetTickCount();
        m_list.insert(node);
        unlock();
        notify();
        return node;
    }

    void Remove(NodeType *node) {
        lock();
        node->removeSelf();
        unlock();
        notify();
    }
    
    EleType Take() {
        lock();
        for (;;) {
            if (m_list.empty()) {
                wait();
                continue;
            }
            NodeType *node = m_list.front();
            uint32_t current = osKernelGetTickCount();
            if (current >= node->priorityValue) {
                EleType ele = node->ele;
                m_free.insert(node);
                unlock();
                return ele;
            }
            uint32_t delayMs = node->priorityValue - current;
            wait(delayMs);
        }
    }

private:
    inline void lock() {
        m_flags.WaitAllFlags(LockFlag);
    }
    inline void unlock() {
        m_flags.SetFlags(LockFlag);
    }

    inline void wait(uint32_t timeout = osWaitForever) {
        unlock();
        m_flags.WaitAllFlags(NotifyFlag, timeout);
        lock();
    }

    inline void notify() {
        m_flags.SetFlags(NotifyFlag);
    }

private:
    utils::PriorityList<EleType, uint32_t> m_list;
    NodeType m_nodes[Capacity];
    NodeType m_free;
    EventFlags m_flags;   
};

}

#endif
